#include "cstanley.h"

int main()
{
    CStanley *objStanley = new CStanley();

    while(objStanley->idxTarget < objStanley->posRef[0].size()-1){
        //在参考路径点上，寻找距离当前点最近的距离，及所对应的索引
        minLatError_idx_t resMinLatError_idx = objStanley->findMinLatError_idx(objStanley->states,
                                                                   objStanley->posRef);

        objStanley->idxTarget = resMinLatError_idx.idx;

        // 计算前轮转角
        double delta = objStanley->stanleyControl(objStanley->idxTarget, objStanley->states,
                                      resMinLatError_idx.latError, objStanley->posRef, objStanley->k);

        // 计算加速度
        double a = objStanley->Kp*(objStanley->targetSpeed - objStanley->states[3])/objStanley->dt;

        // 更新状态量
        vector<double> newStates = objStanley->updateStates(a, objStanley->states,
                                                            delta, objStanley->dt, objStanley->L);
        objStanley->states = newStates;

        // 保存newStates
        objStanley->statesActual.push_back(newStates);

    }

    for(size_t i = 0; i < objStanley->statesActual.size(); i++){
        for(size_t j = 0; j < objStanley->statesActual[0].size()-2; j++){
            if(j == 0)
                objStanley->XActual.push_back(objStanley->statesActual[i][j]);
            else
                objStanley->YActual.push_back(objStanley->statesActual[i][j]);
        }
    }

    objStanley->plotRes(objStanley->XRef, objStanley->YRef,
                        objStanley->XActual, objStanley->YActual);

    delete objStanley;

    return 0;
}
